#ifndef __mml_vector_h__
#define __mml_vector_h__

#include <iosfwd>
#include <sstream>

#include <mml/util.h>
#include <mml/primitives.h>

#include <mst/Vector2.h>
#include <mst/Vector3.h>
#include <mst/Vector4.h>

namespace mml {

  template<int N, typename Type>
  class Vector
  {
    Type m_data[N];

  public:

    /** Default constructor. */
    Vector(Type const& x = Type(),
           Type const& y = Type(),
           Type const& z = Type(),
           Type const& u = Type(),
           Type const& v = Type(),
           Type const& w = Type())
    {
      if (N > 0) {
        m_data[0] = x;
        if (N > 1) {
          m_data[1] = y;
          if (N > 2) {
            m_data[2] = z;
            if (N > 3) {
              m_data[3] = u;
              if (N > 4) {
                m_data[4] = v;
                if (N > 5) {
                  m_data[5] = w;
                  for (int i = 6; i < N; ++i) {
                    m_data[i] = Type(0.0);
                  }
                }
              }
            }
          }
        }
      }
    }

    /** Copy constructor. */
    Vector(Vector const& other)
    {
      for (int i = 0; i < N; ++i) {
        m_data[i] = other.m_data[i];
      }
    }

    /** Destructor. */
    ~Vector()
    {}

    /** Copy assignment operator. */
    Vector const& operator=(Vector const& other)
    {
      new (this) Vector(other);
      return *this;
    }

    /** Raw data constructor. */
    Vector(Type const* data)
    {
      vector_copy(N, m_data, data);
    }

    /** mst::Vector2 constructor */
    Vector(mst::Vector2<Type> const& mstVec)
    {
#ifdef _DEBUG
      if (N != 2) {
        throw mml::SizeMismatch();
      }
#endif
      m_data[0] = mstVec[0];
      m_data[1] = mstVec[1];
    }

    /** mst::Vector3 constructor */
    Vector(mst::Vector3<Type> const& mstVec)
    {
#ifdef _DEBUG
      if (N != 3) {
        throw mml::SizeMismatch();
      }
#endif
      m_data[0] = mstVec[0];
      m_data[1] = mstVec[1];
      m_data[2] = mstVec[2];
    }

    /** mst::Vector4 constructor */
    Vector(mst::Vector4<Type> const& mstVec)
    {
#ifdef _DEBUG
      if (N != 4) {
        throw mml::SizeMismatch();
      }
#endif
      m_data[0] = mstVec[0];
      m_data[1] = mstVec[1];
      m_data[2] = mstVec[2];
      m_data[3] = mstVec[3];
    }

    /** Convert string to vector. */
    void fromString(std::string const& string)
    {
      std::istringstream istr(string);
      istr >> *this;
    }

    /** Convert to string. */
    std::string toString() const
    {
      std::ostringstream ostr;
      ostr << *this;
      return ostr.str();
    }

    /** Internal data. */
    Type const* getData() const
    {
      return m_data;
    }

    /** Non-const internal data. */
    Type* getData()
    {
      return const_cast<Type*>(static_cast<Vector const&>(*this).getData());
    }

    /** Index operator. */
    Type const& operator()(int i) const
    {
#ifdef _DEBUG
      if (i < 0 || i >= N) {
        throw mml::IndexOutOfBounds();
      }
#endif
      return m_data[i];
    }

    /** Non-const index operator. */
    Type& operator()(int i)
    {
      return const_cast<Type&>(static_cast<Vector const&>(*this)(i));
    }

    /** Add a vector to this one. */
    void add(Vector const& other)
    {
      for (int i = 0; i < N; ++i) {
        m_data[i] += other.m_data[i];
      }
    }

    /** Subtract a vector from this one. */
    void subtract(Vector const& other)
    {
      for (int i = 0; i < N; ++i) {
        m_data[i] -= other.m_data[i];
      }
    }

    /** Scale this vector. */
    void scale(Type const& s)
    {
      for (int i = 0; i < N; ++i) {
        m_data[i] *= s;
      }
    }

    /** Negate a vector. */
    void negate()
    {
      for (int i = 0; i < N; ++i) {
        m_data[i] = -m_data[i];
      }
    }

    /** Provide *a* normal vector in a numerical stable way */
    Vector normal() const
    {
      Vector b;  // new vector
      vector_normal(N, m_data, b.m_data);
      return b;
    }

    /** Maximum element. */
    Type max() const
    {
      return vector_max(N, m_data);
    }

    /** Minimum element. */
    Type min() const
    {
      return vector_min(N, m_data);
    }

    /** Squared norm. */
    Type normsq() const
    {
      Type ret = 0.0;
      for (int i = 0; i < N; ++i) {
        ret += m_data[i] * m_data[i];
      }
      return ret;
    }

    /** Vector norm. */
    Type norm() const
    {
      return sqrt(normsq());
    }

    /** Infinity norm. */
    Type norminf() const
    {
      return vector_norminf(N, m_data);
    }

    /** Normalize vector. */
    void normalize()
    {
      Type nrmsq = normsq();
#ifdef _DEBUG
      if (nrmsq == Type(0.0f)) {
        throw mml::DivisionByZero();
      }
#endif
      for (int i = 0; i < N; ++i) {
        m_data[i] /= sqrt(nrmsq);
      }
    }

    /** Reset vector. */
    void reset()
    {
      for (int i = 0; i < N; ++i) {
        m_data[i] = Type(0.0f);
      }
    }

    /** Inner product. */
    Type operator*(Vector const& other) const
    {
      Type ip = Type(0.0f);
      for (int i = 0; i < N; ++i) {
        ip += m_data[i] * other.m_data[i];
      }
      return ip;
    }
  };

  // typedefs:
  typedef Vector<2, float> Vector2f;
  typedef Vector<2, double> Vector2d;
  typedef Vector<3, float> Vector3f;
  typedef Vector<3, double> Vector3d;
  typedef Vector<4, float> Vector4f;
  typedef Vector<4, double> Vector4d;
  typedef Vector<6, float> Vector6f;
  typedef Vector<6, double> Vector6d;


  /** Comparison. */
  template <int N, typename Type>
  bool operator==(Vector<N, Type> const& v1, Vector<N, Type> const& v2)
  {
    for (int i = 0; i < N; ++i) {
      if (v1(i) != v2(i)) {
        return false;
      }
    }
    return true;
  }

  /** Comparison. */
  template <int N, typename Type>
  bool operator!=(Vector<N, Type> const& v1, Vector<N, Type> const& v2)
  {
    return !(v1 == v2);
  }

  /** Angle between vectors. */
  template <int N, typename Type>
  Type angle(Vector<N, Type> const& v1, Vector<N, Type> const& v2)
  {
    return vector_angle(N, v1.getData(), v2.getData());
  }

  /** In-place addition. */
  template <int N, typename Type>
  Vector<N, Type> const& operator+=(Vector<N, Type>& v1, Vector<N, Type> const& v2)
  {
    v1.add(v2);
    return v1;
  }

  /** In-place subtraction. */
  template <int N, typename Type>
  Vector<N, Type> const& operator-=(Vector<N, Type>& v1, Vector<N, Type> const& v2)
  {
    v1.subtract(v2);
    return v1;
  }

  /** In-place scalar multiplication. */
  template <int N, typename Type>
  Vector<N, Type> const& operator*=(Vector<N, Type>& v, Type const& s)
  {
    v.scale(s);
    return v;
  }

  /** In-place scalar division. */
  template <int N, typename Type>
  Vector<N, Type> const& operator/=(Vector<N, Type>& v, Type const& s)
  {
    v.scale(Type(1.0f) / s);
    return v;
  }

  /** Addition */
  template <int N, typename Type>
  Vector<N, Type> operator+(Vector<N, Type>const& v1, Vector<N, Type> const& v2)
  {
    Vector<N, Type> ret(v1);
    ret += v2;
    return ret;
  }

  /** Subtraction */
  template <int N, typename Type>
  Vector<N, Type> operator-(Vector<N, Type> const& v1, Vector<N, Type> const& v2)
  {
    Vector<N, Type> ret(v1);
    ret -= v2;
    return ret;
  }

  /** Postfix scalar multiplication. */
  template <int N, typename Type>
  Vector<N, Type> operator*(Vector<N, Type> const& v, Type const& s)
  {
    Vector<N, Type> ret(v);
    ret.scale(s);
    return ret;
  }

  /** Prefix scalar multiplication. */
  template <int N, typename Type>
  Vector<N, Type> operator*(Type const& s, Vector<N, Type> const& v)
  {
    Vector<N, Type> ret(v);
    ret.scale(s);
    return ret;
  }

  /** Postfix scalar division. */
  template <int N, typename Type>
  Vector<N, Type> operator/(Vector<N, Type> const& v, Type const& s)
  {
#ifdef _DEBUG
    if (s == Type(0.0f)) {
      throw mml::DivisionByZero();
    }
#endif
    Vector<N, Type> ret(v);
    ret.scale(Type(1.0f) / s);
    return ret;
  }

  /** Unary minus */
  template <int N, typename Type>
  Vector<N, Type> operator-(Vector<N, Type> const& v)
  {
    Vector<N, Type> ret(v);
    ret.negate();
    return ret;
  }

  /** Cross product. */
  template <typename Type>
  inline mml::Vector<3, Type> cross(mml::Vector<3, Type> const& a, mml::Vector<3, Type> const& b)
  {
    return mml::Vector<3, Type>(a(1) * b(2) - a(2) * b(1), a(2) * b(0) - a(0) * b(2), a(0) * b(1) - a(1) * b(0));
  }

  /** Serialization. */
  template <int N, typename Type>
  std::ostream& operator<<(std::ostream& s, mml::Vector<N, Type> const& v)
  {
    vector_print(N, v.getData(), s);
    return s;
  }

  /** Deserialization. */
  template <int N, typename Type>
  std::istream& operator>>(std::istream& s, mml::Vector<N, Type>& v)
  {
    vector_read(N, v.getData(), s);
    return s;
  }
}

#endif
